#include <pkgconf/kernel.h>
#include <pkgconf/hal.h>

#include <cyg/kernel/sched.hxx>
#include <cyg/kernel/thread.hxx>
#include <cyg/kernel/thread.inl>
#include <cyg/kernel/mutex.hxx>
#include <cyg/kernel/sema.hxx>
#include <cyg/kernel/flag.hxx>
#include <cyg/kernel/sched.inl>
#include <cyg/kernel/clock.hxx>
#include <cyg/kernel/clock.inl>
#include <cyg/kernel/kapi.h>

#include <cyg/hal/hal_arch.h>
#include <cyg/infra/diag.h>

#include <stdio.h>

/* ----------------------------- APLICACAO ------------------------------- */

#include "sr.h"


#define STACK_SIZE 	CYGNUM_HAL_STACK_SIZE_TYPICAL

/* APLICACAO ------------------------------- */

static int overhead;
static long rtc_resolution[] = CYGNUM_KERNEL_COUNTERS_RTC_RESOLUTION;
static long ns_per_system_clock;
static long durations[100];

#ifdef CYGSEM_HAL_STOP_CONSTRUCTORS_ON_FLAG
externC void
cyg_hal_invoke_constructors();
#endif

/* ----------------------------- COMUNICACAO REDE ------------------------------- */


static cyg_handle_t plantHandle;
static cyg_thread plantData;
static char plantStack[STACK_SIZE];

static cyg_handle_t genStimulusHandle;
static cyg_thread genStimulusData;
static char genStimulusStack[STACK_SIZE];

static cyg_handle_t craneSystemHandle;
static cyg_thread craneSystemData;
static char craneSystemStack[STACK_SIZE];

// Resultado

void
wait_for_tick(void)
{
    cyg_tick_count_t tv0, tv1;
    tv0 = cyg_current_time();
    while (true) {
        tv1 = cyg_current_time();
        if (tv1 != tv0) break;
    }
}

long long
ticks2us(cyg_uint32 ticks)
{
    long long ns;
    ns = (ns_per_system_clock * (long long) ticks) / CYGNUM_KERNEL_COUNTERS_RTC_PERIOD;
    ns += 5;  // for rounding to .01us
    return ns;
}

cyg_uint32 starttime, endtime;
cyg_tick_count_t starttick, endtick;

long long
getDuration(cyg_tick_count_t starttick, cyg_tick_count_t endtick, 
        cyg_uint32 starttime, cyg_uint32 endtime) {

    cyg_uint32 ticks;

    if(starttick == endtick) {
        ticks = endtime - starttime;
    }
    else {
    	ticks = (endtick - starttick - 1) * CYGNUM_KERNEL_COUNTERS_RTC_PERIOD +
    	    endtime + CYGNUM_KERNEL_COUNTERS_RTC_PERIOD - starttime;
    }

    return ticks2us(ticks - overhead);
}

cyg_flag_t hasEvent;

/* bit 0 : alfa
   bit 1 : xl
   bit 2 : xc
   bit 4 : powerOn
   bit 5 : shutDown
   bit 6 : posdesired  */
int hasEventFlags;             // para indicar quando esta absent ou present 
                               // implementando o hasToken
char Brake;
double VC;

double Fd;

double Alfa;
double XL;
double XC;

double posdesired;

externC void initCrane(void);
externC void crane(void);

static void
craneSystem(cyg_addrword_t p) {
int i, count = 0;
cyg_uint32 tv[32], tv0;
long ave, dev;

    // descobre o overhead da leitura do clock
    wait_for_tick();
    for (i = 0;  i < 32;  i++) {
        HAL_CLOCK_READ(&tv[i]);
    }
    tv0 = 0;
    for (i = 1;  i < 32;  i++) {
        tv0 += tv[i] - tv[i-1];
    }
    overhead = tv0 / (32-1); 
    ns_per_system_clock = 1000000/rtc_resolution[1];
    
    // Inicializacoes
    initCrane();

    for(;;) {
        cyg_flag_wait(&hasEvent, 1, CYG_FLAG_WAITMODE_OR | CYG_FLAG_WAITMODE_CLR);

        HAL_CLOCK_READ(&starttime);
        starttick = cyg_current_time();

        crane();
        hasEventFlags = 0;

        HAL_CLOCK_READ(&endtime);
        endtick = cyg_current_time();

        if(count < 100) {
          durations[count] = getDuration(starttick, endtick, starttime, endtime);
          //diag_printf("Duration = %5d.%02d\n", (int)(durations[count]/1000),
           //         (int)((durations[count]%1000)/10));
          diag_printf("duration = %d\n", durations[count]);
          count++;
        }
    }
}

static void
genStimulus(cyg_addrword_t p) {

    Fd = 0.0;
    SET_BIT_TRUE(hasEventFlags, POWERONBIT);

    cyg_flag_setbits(&hasEvent, 1);
    cyg_thread_delay(10);

    posdesired = 3.5;
    SET_BIT_TRUE(hasEventFlags, POSDESIREDBIT);

    cyg_flag_setbits(&hasEvent, 1);
    cyg_thread_delay(200);
}

#define MC 10
#define ML 100
#define G  9.81
#define DC 0.5
#define R 5.0
#define DL 0.01
#define STEP 0.002

void
_diff(double *state) {
    double xdot, x, dxdot_dt, dx_dt;
    double alphadot, alpha, dalphadot_dt, dalpha_dt;
    double fc, dfc_dt;

    if(Brake) {
        xdot = 0.0;
    }
    else {
        xdot = state[0];
    }

    x = state[1];
    alphadot = state[2];
    alpha = state[3];
    fc = state[4];

    dxdot_dt = (fc+ G*ML*alpha - DC*xdot)/MC;
    dx_dt = xdot;
    dalphadot_dt = (-G*alpha + (-G*alpha*ML + DC*xdot -fc)/MC + 
        (-DL*xdot -R*DL*alphadot + Fd)/ML)/R;
    dalpha_dt = alphadot;
    dfc_dt = (4 * VC) - fc;

    state[0] = dxdot_dt;
    state[1] = dx_dt;
    state[2] = dalphadot_dt;
    state[3] = dalpha_dt;
    state[4] = dfc_dt;
}

static void
plant(cyg_addrword_t p) {
    double k1[5], k2[5], k3[5], k4[5];
    double state[5];
    int i;

    for(i = 0;i < 5;i++) state[i] = 0.0;

    for(;;) {
        for(i = 0; i < 5; i++) k1[i] = state[i];
        _diff(k1);
        for(i = 0; i < 5; i++) k1[i]= k1[i] * STEP;

        for(i = 0; i < 5; i++) k2[i] = state[i] + (k1[i]/2);
        _diff(k2);
        for(i = 0; i < 5; i++) k2[i]= k2[i] * STEP;

        for(i = 0; i < 5; i++) k3[i]= state[i]+ (k2[i]/2);
        _diff(k3);
        for(i = 0; i < 5; i++) k3[i] = k3[i] * STEP;

        for(i = 0; i < 5; i++) k4[i]= state[i] + k3[i];
        _diff(k4);
        for(i = 0; i < 5; i++) k4[i] = k4[i] * STEP;

        for(i = 0; i < 5; i++) k1[i] = (k1[i]+ (2*k2[i]) + (2*k3[i])+ k4[i])/6;
        for(i = 0; i < 5; i++) state[i] = state[i] + k1[i];

        XC = state[1];
        Alfa = state[3];
        XL = state[1] + R * state[3];

        SET_BIT_TRUE(hasEventFlags, ALFABIT);
        SET_BIT_TRUE(hasEventFlags, XLBIT);
        SET_BIT_TRUE(hasEventFlags, XCBIT);

        cyg_flag_setbits(&hasEvent, 1);
        cyg_thread_delay(2);
    }
}


/* APLICACAO ------------------------------- */

/* ----------------------------- MAIN ------------------------------- */


#ifdef CYGSEM_HAL_STOP_CONSTRUCTORS_ON_FLAG
externC void
cyg_hal_invoke_constructors();
#endif

externC
void cyg_start( void )
{
#ifdef CYGSEM_HAL_STOP_CONSTRUCTORS_ON_FLAG
    cyg_hal_invoke_constructors();
#endif

    cyg_flag_init(&hasEvent);

    hasEventFlags = 0;

    Brake = 1;
    VC = 0.0;
    Fd = 0.0;

    Alfa = 0.0;
    XL = 0.0;
    XC = 0.0;

    posdesired = 0.0;

    cyg_thread_create(10, plant, (cyg_addrword_t) 0, "Plant",
        &plantStack[0], STACK_SIZE, &plantHandle, &plantData);
    cyg_thread_resume(plantHandle);
    
    cyg_thread_create(10, genStimulus, (cyg_addrword_t) 0, "GenStimulus",
        &genStimulusStack[0], STACK_SIZE, &genStimulusHandle, &genStimulusData);
    cyg_thread_resume(genStimulusHandle);

    cyg_thread_create(10, craneSystem, (cyg_addrword_t) 0, "CraneSystem",
        &craneSystemStack[0], STACK_SIZE, &craneSystemHandle, &craneSystemData);
    cyg_thread_resume(craneSystemHandle);

    Cyg_Scheduler::scheduler.start();
}
